package com.testcolision.simulation;

import com.badlogic.gdx.math.Quaternion;
import com.badlogic.gdx.math.Vector3;

public class RigidBody {

	public float inertia;
	public float mass;
	public float inverseMass;
	public float inverseInertia;

	public RigidBodyState[] rigidBodyStates = new RigidBodyState[2];
	public int sourceStateIndex = 0;
	public int targetStateIndex = 1;

	public Vector3[] rigidBodyPoints = new Vector3[8];

	public float coefficientRestitution;

	public Vector3 lastForce = new Vector3();

	public RigidBody(Vector3 halfsize, float mass) {
		
		//Initialization
		this.mass = mass;
		this.inverseMass = 1 / mass;

		this.inertia = (1.0f / 12.0f) * mass * (halfsize.x * halfsize.x) * (halfsize.z * halfsize.z);
		this.inverseInertia = 1 / inertia;
		rigidBodyStates[0] = new RigidBodyState();
		rigidBodyStates[1] = new RigidBodyState();

		rigidBodyStates[0].position.set(0, 0, 0);
		rigidBodyStates[0].orientation.set(0, 0, 0, 1);
		rigidBodyStates[0].angularVelocity.set(0, 0, 0);
		rigidBodyStates[0].torque_accumulator.set(0, 0, 0);
		
		rigidBodyStates[1].position.set(0, 0, 0);
		rigidBodyStates[1].orientation.set(0, 0, 0, 1);
		rigidBodyStates[1].angularVelocity.set(0, 0, 0);
		rigidBodyStates[1].torque_accumulator.set(0, 0, 0);
		
		rigidBodyPoints[0] = new Vector3(halfsize.x, 0, -halfsize.z);
		rigidBodyPoints[1] = new Vector3(-halfsize.x, 0, -halfsize.z);
		rigidBodyPoints[2] = new Vector3(-halfsize.x, 0, halfsize.z);
		rigidBodyPoints[3] = new Vector3(halfsize.x, 0, halfsize.z);
		rigidBodyPoints[4] = new Vector3(halfsize.x, halfsize.y * 2, -halfsize.z);
		rigidBodyPoints[5] = new Vector3(-halfsize.x, halfsize.y * 2, -halfsize.z);
		rigidBodyPoints[6] = new Vector3(-halfsize.x, halfsize.y * 2, halfsize.z);
		rigidBodyPoints[7] = new Vector3(halfsize.x, halfsize.y * 2, halfsize.z);

		
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < rigidBodyStates[1].boundingPoints.length; j++) {
				rigidBodyStates[i].boundingPoints[j] = new Vector3();
			}
		}
		
		coefficientRestitution = 0.3f;
	}

	public void setLocation(Vector3 position, Quaternion orientation) {
		rigidBodyStates[0].position.set(position);
		rigidBodyStates[0].orientation.set(orientation);
	}

	

	public Vector3 localToWorld(Vector3 local) {
		return local.cpy().mul(rigidBodyStates[targetStateIndex].localToWorldMatrix);
	}

	public Vector3 worldToLocal(Vector3 world) {
		return world.cpy().mul(rigidBodyStates[targetStateIndex].worldToLocalMatrix);
	}

	public void addForce(Vector3 worldForce, Vector3 worldPoint) {
		RigidBodyState currentState = rigidBodyStates[sourceStateIndex];
		Vector3 centerToPoint = new Vector3().set(worldPoint.cpy().sub(currentState.position));

		currentState.force_accumulator.add(worldForce);
		currentState.torque_accumulator.add(centerToPoint.crs(worldForce.cpy()));
	}

	public Vector3 pointVelocity(Vector3 worldPoint) {
		RigidBodyState currentState = rigidBodyStates[targetStateIndex];
		Vector3 pointPos = new Vector3().set(worldPoint.cpy().sub(currentState.position));
		Vector3 pointVel = new Vector3().set(currentState.linearVelocity).add(currentState.angularVelocity.cpy().crs(pointPos));
		return pointVel;
	}

	public Vector3 positionToCMPerp(Vector3 worldPoint) {
		RigidBodyState currentState = rigidBodyStates[targetStateIndex];
		Vector3 pointPos = new Vector3().set(worldPoint.cpy().sub(currentState.position));
		return new Vector3().set(currentState.angularVelocity.cpy().crs(pointPos));
	}
}
